/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include "voslamdef.h"

/*  */
void match_corners(double *res,double *strength, int  *matches, BASETYPE *patchesA, BASETYPE *patchesB,int *tmp, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th);
void match_cornerc(BASETYPE2 *strength, int  *matches, BASETYPE2 *patchA, BASETYPE2 *patchesB, double *featureA,double *featuresB, int num_featuresB,int wxy, int th);
void match_cornersc(BASETYPE2 *res,BASETYPE2 *strength, int  *matches, BASETYPE2 *patchesA, BASETYPE2 *patchesB,int *tmp, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th);
/** finds unique best matches for normalised patches using crosscorrelation
* @param res[*] num_featuresAxnum_featuresB helper array
* @param *strenght [out] num_featuresAx1 array, array element is best match score
* @param *matches [out] num_featuresAx1 array, array element i is index+1 of matching patchesB to ith patchesB or zero if no match found
* @param *patchesA [out] wxyxnum_featuresA normalised image patches
* @param *patchesB [out] wxyxnum_featuresB normalised image patches
* @param *tmp [*] num_featuresA x1 helper array
* @param *featuresA [in] 3xnum_featuresA patch centers coming from for example sp_find_loc_max_4tile
* @param *featuresB [in] 3xnum_featuresB patch centers coming from for example sp_find_loc_max_4tile
* @param *num_featuresA [in] number of patchesA
* @param *num_featuresB [in] number of patchesB
* @param wxy [in] length of patch array
* @param th [in] maximum distance along image coordinates for match
*/
void match_cornerscl(BASETYPE2 *res,BASETYPE2 *strength, int  *matches, BASETYPE2 *patchesA, BASETYPE2 *patchesB,int *tmp, double *featuresA,double *featuresB,int numFeaturesA, int numFeaturesB,int wxy, int th,float th1,float th2);
void match_cornerscE(double *E,BASETYPE2 *resl, int  *matchl,BASETYPE2 *reso, int  *matcho, BASETYPE2 *patchesA, BASETYPE2 *patchesB, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th);
/**/
void detect_corners(BASETYPE *im,BASETYPE1 *quality,BASETYPE1 *w,int M,int N);
/** calculates pixelwise corner quality (smaller eigenvalue of local image gradient covariance matrix) image for image
* @param *im [in] MxN grayscale image
* @param *quality [out] MxN corner quality image
* @param *w [in] Mx15 helper array
* @param M [in] pixels in scanline
* @param N [in] number of scanlines
*/
void detect_corners_tile(BASETYPE *im,BASETYPE1 *quality,BASETYPE1 *w,int M,int stride,int N);
/** calculates pixelwise corner quality (smaller eigenvalue of local image gradient covariance matrix) image for imagetile
* @param *im [in] MxN grayscale image
* @param *quality [out] MxN corner quality image
* @param *w [in] Mx15 helper array
* @param M [in] pixels in tile scanline
* @param stride [in] pixels in scanline
* @param N [in] number of scanlines
*/
void find_loc_max2(BASETYPE1 *q,double *f,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int max_num_of_features);
/** calculates local maximas in quality image
* @param *q[in] MxN corner quality image
* @param *f [out] 3xk1(max max_num_of_features) maxima 2D position and value in corner quality image
* @param Th [in] threshold of minumum value for accepted local maxima
* @param *tmp [in] Mx1 helper array
* @param *ix [in] Mx1 helper array
* @param *indx [in] Mx1 helper array
* @param k1 [out] number of found maxima smaller or equal to maxNumOfFeatures
* @param k2 [out] statistics number local maximas greater than 0.2*global maxima 
* @param M [in] pixels in scanline
* @param N [in] number of scanlines
* @param max_num_of_features [in] maximum number of maximas to be returned
*/
void sp_find_loc_max2(BASETYPE1 *q,double *f,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int max_num_of_features);
/** calculates local maximas in quality image with subpixel accuracy
* @param *q[in] MxN corner quality image
* @param *f [out] 3xk1(max max_num_of_features) maxima 2D position and value in corner quality image
* @param Th [in] threshold of minumum value for accepted local maxima
* @param *tmp [in] Mx1 helper array
* @param *ix [in] Mx1 helper array
* @param *indx [in] Mx1 helper array
* @param k1 [out] number of found maxima smaller or equal to maxNumOfFeatures
* @param k2 [out] statistics number local maximas greater than 0.2*global maxima 
* @param M [in] pixels in scanline
* @param N [in] number of scanlines
* @param max_num_of_features [in] maximum number of maximas to be returned
*/
void sp_find_loc_max_tile(BASETYPE1 *q,double *fea,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *k1, int *k2, int M, int N, int oM, int oN, int stride, int maxNumOfFeatures);
void sp_find_loc_max_4tile(BASETYPE1 *q,double *fea,BASETYPE1 Th,int *tmp,BASETYPE1 *ix,int *indx, int  *num_fea, int *k2, int M, int N, int max_num_of_features);
/** calculates local maximas in quality image with subpixel accuracy
* @param *q[in] MxN corner quality image
* @param *f [out] 3xk1(max max_num_of_features) maxima 2D position and value in corner quality image
* @param Th [in] threshold of minumum value for accepted local maxima
* @param *tmp [in] Mx1 helper array
* @param *ix [in] Mx1 helper array
* @param *indx [in] Mx1 helper array
* @param k1 [out] number of found maxima smaller or equal to maxNumOfFeatures
* @param k2 [out] statistics number local maximas greater than 0.2*global maxima 
* @param M [in] pixels in scanline
* @param N [in] number of scanlines
* @param max_num_of_features [in] maximum number of maximas to be returned
*/
void undist_points3(double *warped, double *pts, double *p, int cyc, int N);
void dist_points3(double *warped, double *pts, double *p, int N);
void normalise_points(double *fA,double *pts,double *p,int N);

void extract_patches(BASETYPE *patches,BASETYPE *im,double *features,int numFeatures,int wx,int wy, int M);
void extract_patchesn(BASETYPE2 *patches, BASETYPE *im, double *points,int numpoints,int wx,int wy, int M);
void sp_extract_patches(BASETYPE *patches,BASETYPE *im,double *features,int num_features,int wx,int wy,int M);
void sp_extract_patchesn(BASETYPE2 *patches,BASETYPE *im,double *features,int num_features,int wx,int wy,int M);
/** extract normalised patches from image with subpixel location
* @param *patches [out] ((2*wx+1)x(2*wy+1))xnum_features image patches
* @param *im [in] MxN  grayscale image
* @param *features [in] 3xnum_features patch centers coming from sp_find_loc_max_4tile, feature+3*i is location along scanline,
   feature+3*i+1 is location across scanline
* @param num_features [in] number of patches to be extracted
* @param wx [in] patches extends +-wx from patch center along scanline
* @param wy [in] patches extends +-wy from patch center across scanlines
* @param M [in] pixels in scanline
*/
void project_landmarks(double *v,double *p,double *pts,double *X,int *maski,int *count,int num_landmarks,int M, int N);
void project_landmarks1(double *v,double *p,double *pts,double *X,int *maski,int *count,int num_landmarks,int M, int N);
void warp_image(BASETYPE *warped,BASETYPE *im,double *u,double *v,double *iu,double *iv,double s,int M,int N);
void warp_image_fast(BASETYPE *warped,BASETYPE *im,double *u,double *v,int M,int N);
void find_nearest_point_to_landmark(double *v,double *p,double *fea,double *X,double *mindist,int *ind,int num_points,int M,int N);
void slam_find_nearest_point_to_landmark(double *v,double *p,double *fea,double *X,double *mindist,int *ind,int num_points,int M,int N);
void match_cornerscf(BASETYPE2 *resl, int  *matchl,BASETYPE2 *reso, int  *matcho, BASETYPE2 *patchesA, BASETYPE2 *patchesB, double *featuresA,double *featuresB,int num_featuresA, int num_featuresB,int wxy, int th);
BASETYPE2 match_features2(BASETYPE2 *im1, BASETYPE2 *im2, int N);
void proj3D(double *v,double *p,double *pts,double *X,int N);
void proj3D1(double *v,double *p,double *pts,double *X,int N);
double match_features1(BASETYPE *im1, BASETYPE *im2, int N);
void pyr_down(BASETYPE *im, BASETYPE *pim,int M, int N);
void orient3D_patch(double *q,double *t,double *n,double *Xc, double dist);
